
.equ CPUID_REGADDR ,0xd0000000

.macro macro_get_cpuid reg
    LDR     \reg, =CPUID_REGADDR
    LDR     \reg, [\reg]
.endm

.macro decl_pendsv_handler name, currvar, nextvar, prevvar
    .global \currvar
    .global \nextvar
    .global \prevvar

    .thumb_func
    .global  \name

    .type \name, %function
    \name:
    CPSID   I
    MRS     R0, PSP
    CMP     R0, #0
    BEQ     1f

    SUBS    R0, R0, #0x20
    STMIA   R0!, {R4 - R7}
    MOV     R4, R8
    MOV     R5, R9
    MOV     R6, R10
    MOV     R7, R11
    STMIA   R0!, {R4-R7}
    SUBS    R0, R0, #0x20

    // The lines should be added to get the index of the current core
    macro_get_cpuid R2
    LSLS     R2, R2, #2

    LDR     R1, =\currvar
    ADD     R1, R1, R2
    LDR     R1, [R1]
    STR     R0, [R1]

    1:
    macro_get_cpuid R2
    LSLS     R2, R2, #2

    LDR     R0, =\currvar
    LDR     R1, =\nextvar
    LDR     R3, =\prevvar
    ADD     R0, R0, R2
    ADD     R1, R1, R2
    ADD     R3, R3, R2

    LDR     R2, [R0]
    STR     R2, [R3]
    LDR     R2, [R1]
    STR     R2, [R0]

    LDR     R0, [R2] // SP recovered to R0

    LDMIA   R0!, {R4 - R7}
    LDMIA   R0!, {R2 - R3}
    MOV     R8, R2
    MOV     R9, R3
    LDMIA   R0!, {R2 - R3}
    MOV     R10, R2
    MOV     R11, R3
    MSR     PSP, R0

    MOV     R0, R14
    MOVS    R1, #0x04
    ORRS    R0, R1
    MOV     R14, R0

    CPSIE   I

    BX      LR
.endm

    .text
    .align 2
    .thumb
    .syntax unified

decl_pendsv_handler PendSV_Handler, k_curr_task, k_next_task, k_prev_task